/*****************************************************************************
 * nv12.c: nv12 input
 *****************************************************************************
 * Copyright (C) 20014-2015 x264 project
 *
 * Authors: Justin <pengtao.kang@ingenic.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02111, USA.
 *
 * This program is also available under a commercial proprietary license.
 * For more information, contact us at licensing@x264.com.
 *****************************************************************************/

#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <pthread.h>
#include <sys/select.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <cliopt.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
#define REQUEST_BUF_NUM			(3)
#define TOTAL_READ_FRAME		(25 * 60)
#define FAIL_IF_ERROR(cond, ...)	FAIL_IF_ERR( cond, "nv12", __VA_ARGS__ )

enum tx_sensor_control_bus_type {
	TX_SENSOR_CONTROL_INTERFACE_I2C = 1,
	TX_SENSOR_CONTROL_INTERFACE_SPI,
};

/* isp core tuning */
enum isp_image_tuning_cmd_id {
	IMAGE_TUNING_CID_CUSTOM_BASE = (V4L2_CID_USER_BASE | 0xe000),
	IMAGE_TUNING_CID_CUSTOM_AE_GAIN = IMAGE_TUNING_CID_CUSTOM_BASE,
	IMAGE_TUNING_CID_CUSTOM_AE_COMP,
	IMAGE_TUNING_CID_CUSTOM_AF_MODE,
	IMAGE_TUNING_CID_CUSTOM_ANTI_FOG,
	IMAGE_TUNING_CID_CUSTOM_RESOLUTION,
	IMAGE_TUNING_CID_CUSTOM_FPS,
	IMAGE_TUNING_CID_CUSTOM_TEST_PATTERN,		//test pattern
	IMAGE_TUNING_CID_CUSTOM_ISP_PROCESS, 		//isp process
	IMAGE_TUNING_CID_CUSTOM_ISP_FREEZE, 		//isp process
	IMAGE_TUNING_CID_CUSTOM_BL,			//black level
	IMAGE_TUNING_CID_CUSTOM_SHAD,			//lens shading
	IMAGE_TUNING_CID_CUSTOM_SINTER_DNS,		//sinter denoise
	IMAGE_TUNING_CID_CUSTOM_TEMPER_DNS,		//temper denoise
	IMAGE_TUNING_CID_CUSTOM_DYNAMIC_DP,		//dynamic defect pixels
	IMAGE_TUNING_CID_CUSTOM_GE,			//green equalist
	IMAGE_TUNING_CID_CUSTOM_STATIC_DP,		//static defect pixels
	IMAGE_TUNING_CID_CUSTOM_DRC,			//raw dynamic range compression
	IMAGE_TUNING_CID_CUSTOM_WDR_FLT,		//WDR companded frontend lookup table
	IMAGE_TUNING_CID_CUSTOM_WDR,			//sharpen
	IMAGE_TUNING_CID_CUSTOM_BUTT,			//the end
};

typedef enum isp_core_temper_mode_enum{
	ISPCORE_TEMPER_MODE_DISABLE,
	ISPCORE_TEMPER_MODE_AUTO,
	ISPCORE_TEMPER_MODE_MANUAL,
} ISP_CORE_TEMPER_MODE;

struct tx_isp_i2c_board_info {
	char type[20];
	int addr;
	int i2c_adapter_id;
};

struct tx_isp_spi_board_info {
	char modalias[32];
	int bus_num;
};

/* define sensor attribute */
struct v4l2_sensor_register_info {
	char name[32];
	enum tx_sensor_control_bus_type cbus_type;
	union {
		struct tx_isp_i2c_board_info i2c;
		struct tx_isp_spi_board_info spi;
	};
	unsigned short rst_gpio;
	unsigned short pwdn_gpio;
	unsigned short power_gpio;
};

struct frame_image_scalercap {
	unsigned short max_width;
	unsigned short max_height;
	unsigned short min_width;
	unsigned short min_height;
};

struct frame_image_scaler {
	unsigned short out_width;
	unsigned short out_height;
};

/* define common struct */
enum tx_isp_priv_ioctl_direction {
	TX_ISP_PRIVATE_IOCTL_SET,
	TX_ISP_PRIVATE_IOCTL_GET,
};

/* isp image tuning */
struct isp_image_tuning_default_ctrl {
	enum tx_isp_priv_ioctl_direction dir;
	struct v4l2_control control;
};

#define VIDIOC_DEFAULT_CMD_LISTEN_BUF	 _IOR('V', BASE_VIDIOC_PRIVATE - 1, int)
#define VIDIOC_ISP_PRIVATE_IOCTL	 _IOW('V', BASE_VIDIOC_PRIVATE, struct isp_private_ioctl)
#define VIDIOC_REGISTER_SENSOR		 _IOW('V', BASE_VIDIOC_PRIVATE + 1, struct v4l2_sensor_register_info)
#define VIDIOC_RELEASE_SENSOR		 _IOW('V', BASE_VIDIOC_PRIVATE + 2, struct v4l2_sensor_register_info)
#define VIDIOC_DEFAULT_CMD_SCALER_CAP	 _IOWR('V', BASE_VIDIOC_PRIVATE + 3, struct frame_image_scalercap)
#define VIDIOC_DEFAULT_CMD_SET_SCALER	 _IOW('V', BASE_VIDIOC_PRIVATE + 4, struct frame_image_scaler)
#define VIDIOC_DEFAULT_CMD_ISP_TUNING	 _IOWR('V', BASE_VIDIOC_PRIVATE + 6, struct isp_image_tuning_default_ctrl)

struct buf {
	void *vaddr;
	void *paddr;
	unsigned int len;
};

struct user_buffer {
	struct buf *buf;
	int buf_cnt;
} ubuf = {NULL, 0};

typedef struct
{
	int vi_fd;
	int tuningfd;
	int chn_index;
	int fd;
	int width, height;
	int i_csp;
	sensor_type_t sensor_type;
	struct v4l2_sensor_register_info sensor_info;
	int next_frame;
} nv12_hnd_t;

static int v4l2_3d_init(nv12_hnd_t *h)
{
	struct v4l2_control control;
	/* create the thread of isp-tuning */
	h->tuningfd = open("/dev/video0", O_RDWR);
	if(h->tuningfd < 0){
		fprintf(stderr, "cannot open '/dev/video0':%d, %s\n", errno, strerror(errno));
		goto open_video0_error;
	}
	/* enable temper module */
	memset(&control, 0, sizeof(struct v4l2_control));
	control.id = IMAGE_TUNING_CID_CUSTOM_TEMPER_DNS;
	control.value = ISPCORE_TEMPER_MODE_AUTO;
	if (ioctl (h->tuningfd, VIDIOC_S_CTRL, &control) != 0) {
		fprintf(stderr, "failed to enable temper module!\n");
		goto err_set_temper_module;
	}

	return 0;

err_set_temper_module:
	close(h->tuningfd);
open_video0_error:
	return -1;
}

static int v4l2_3d_deinit(nv12_hnd_t *h)
{
	return close(h->tuningfd);
}

static int v4l2_para_init(nv12_hnd_t *h, char *file_name)
{
	int ret = 0;
	struct v4l2_fmtdesc fmtdesc;
	struct v4l2_cropcap cropcap;
	struct v4l2_crop crop;
	struct frame_image_scalercap scalercap;
	struct frame_image_scaler scaler;
	struct v4l2_format fmt;

	fmtdesc.index = 0;
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	while (ioctl(h->fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
		printf("%s:index = %d, pixelformat = 0x%08x, description=%s\n",
				file_name, fmtdesc.index, fmtdesc.pixelformat, fmtdesc.description);
		fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmtdesc.index++;
	}

	/* capture crop info */
	if (ioctl(h->fd, VIDIOC_CROPCAP, &cropcap) == 0) {
		/* set crop */
		crop.type = cropcap.type;
		crop.c.top = 0;
		crop.c.left = 0;
		crop.c.width = h->width;
		crop.c.height = h->height;
		if (ioctl(h->fd, VIDIOC_S_CROP, &crop) == 0) {
			printf("%s:crop.left = %d crop.top = %d\n crop.width = %d crop.height = %d\n",
					file_name, crop.c.left, crop.c.top, crop.c.width, crop.c.height);
		} else {
			fprintf (stderr, "%s: VIDIOC_S_CROP error\n", file_name);
			goto err_VIDIOC_S_CROP;
		}
	} else {
		fprintf (stderr, "%s: VIDIOC_CROPCAP error\n", file_name);
		goto err_VIDIOC_CROPCAP;
	}

	/* capture scaler info*/
	if ((ret = ioctl(h->fd, VIDIOC_DEFAULT_CMD_SCALER_CAP, &scalercap)) == 0) {
		printf("%s:scalercap.maxwidth = %d scalercap.maxheight = %d\n"
				"scalercap.minwidth = %d scalercap.minheight = %d &scalercap = %p\n",
				file_name, scalercap.max_width, scalercap.max_height,
				scalercap.min_width, scalercap.min_height, &scalercap);
		/* set scaler */
		scaler.out_width = scalercap.max_width - h->chn_index * 360;
		scaler.out_height = scalercap.max_height - h->chn_index * 240;
		if ((ret = ioctl(h->fd, VIDIOC_DEFAULT_CMD_SET_SCALER, &scaler)) == 0) {
			printf("%s:scaler.outwidth = %d scaler.outheight = %d\n",
					file_name, scaler.out_width, scaler.out_height);
		}else{
			fprintf(stderr, "%s: VIDIOC_SET_SCALER error\n", file_name);
			goto err_VIDIOC_SET_SCALER;
		}
	}

	/* set fmt */
	memset(&fmt, 0, sizeof(struct v4l2_format));
	if(ret == 0){
		printf("use scaler resolution:%dx%d\n", scaler.out_width, scaler.out_height);
		/* scaler success */
		fmt.fmt.pix.width = scaler.out_width;
		fmt.fmt.pix.height = scaler.out_height;
	}else{
		printf("use crop resolution:%dx%d\n", crop.c.width, crop.c.height);
		fmt.fmt.pix.width = crop.c.width;
		fmt.fmt.pix.height = crop.c.height;
	}
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.field = V4L2_FIELD_ANY;
	if (ioctl(h->fd, VIDIOC_TRY_FMT, &fmt) == 0) {
		if (ioctl(h->fd, VIDIOC_S_FMT, &fmt) == 0) {
			printf("%s: frm resolution:width = %d height = %d\n", file_name, fmt.fmt.pix.width, fmt.fmt.pix.height);
		} else {
			fprintf(stderr, "%s: VIDIOC_S_FMT error\n", file_name);
			goto err_VIDIOC_S_FMT;
		}
	} else {
		fprintf(stderr, "%s: VIDIOC_TRY_FMT error\n", file_name);
		goto err_VIDIOC_TRY_FMT;
	}

	return 0;

err_VIDIOC_TRY_FMT:
err_VIDIOC_S_FMT:
err_VIDIOC_SET_SCALER:
err_VIDIOC_CROPCAP:
err_VIDIOC_S_CROP:
	return -1;
}

static int v4l2_init_ubuf(nv12_hnd_t *h)
{
	struct v4l2_requestbuffers v4l2_rbuf;
	struct v4l2_buffer v4l2_buf;
	enum v4l2_buf_type buf_type;
	int i = 0, bufsize = 0, ready_bufs = 0;
	void *bufbase_v = NULL, *bufbase_p = NULL;

	memset(&v4l2_rbuf, 0, sizeof(struct v4l2_requestbuffers));
	v4l2_rbuf.count = REQUEST_BUF_NUM;
	v4l2_rbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	v4l2_rbuf.memory = V4L2_MEMORY_USERPTR;
	if (ioctl(h->fd, VIDIOC_REQBUFS, &v4l2_rbuf) < 0) {
		fprintf(stderr, "request buffer as userptr failed:%s\n", strerror(errno));
		goto err_reqbufs;
	}

	if ((ubuf.buf = (struct buf *)malloc(v4l2_rbuf.count * sizeof(struct buf))) == NULL) {
		fprintf(stderr, "malloc mybuf failed:%s\n", strerror(errno));
		goto err_buf_malloc;
	}
	ubuf.buf_cnt = v4l2_rbuf.count;
	bufsize = h->width*h->height*3/2;
	if ((bufbase_v = c_kalloc(-1 ,bufsize * v4l2_rbuf.count, C_VB_ALIGN)) == NULL) {
		fprintf(stderr, "c_kalloc bufbase_v failed\n");
		goto err_bufbase_v_c_kalloc;
	}

	bufbase_p = (void *)c_kvirt_to_kphys((intptr_t)bufbase_v);

	for (i = 0; i < v4l2_rbuf.count; i++) {
		ubuf.buf[i].len = bufsize;
		ubuf.buf[i].vaddr = bufbase_v + bufsize * i;
		ubuf.buf[i].paddr = bufbase_p + bufsize * i;
	}

	for (i = 0; i < v4l2_rbuf.count; i++) {
		memset(&v4l2_buf, 0, sizeof(struct v4l2_buffer));
		v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		v4l2_buf.memory = V4L2_MEMORY_USERPTR;
		v4l2_buf.index = i;
		v4l2_buf.m.userptr = (unsigned long)(ubuf.buf[i].paddr);
		v4l2_buf.length = ubuf.buf[i].len;

		if (ioctl(h->fd, VIDIOC_QBUF, &v4l2_buf) < 0) {
			fprintf(stderr, "quene requested buf into quene failed:%s\n", strerror(errno));
			goto err_qbuf;
		}
	}

	buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (ioctl(h->fd, VIDIOC_STREAMON, &buf_type) < 0) {
		fprintf(stderr, "start stream of camera failed:%s\n", strerror(errno));
		goto err_streamon;
	}

	return 0;

err_streamon:
	i = ubuf.buf_cnt;
err_qbuf:
	for (--i; i >= 0; i--) {
		v4l2_buf.index = i;
		if (ioctl(h->fd, VIDIOC_DEFAULT_CMD_LISTEN_BUF, &ready_bufs) >= 0) {
			ioctl(h->fd, VIDIOC_DQBUF, &v4l2_buf);
		}
	}
	c_kfree(-1, ubuf.buf[0].vaddr);
err_bufbase_v_c_kalloc:
	free(ubuf.buf);
err_buf_malloc:
err_reqbufs:
	return -1;
}

static int v4l2_deinit_ubuf(nv12_hnd_t *h)
{
	int i = 0, ready_bufs = 0;
	struct v4l2_buffer v4l2_buf;
	enum v4l2_buf_type buf_type;

	for (i = ubuf.buf_cnt - 1; i >= 0; i--) {
		if (ioctl(h->fd, VIDIOC_DEFAULT_CMD_LISTEN_BUF, &ready_bufs) >= 0) {
			memset(&v4l2_buf, 0, sizeof(struct v4l2_buffer));
			v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			v4l2_buf.memory = V4L2_MEMORY_USERPTR;
			v4l2_buf.index = i;
			v4l2_buf.m.userptr = (unsigned long)(ubuf.buf[i].paddr);
			v4l2_buf.length = ubuf.buf[i].len;
			ioctl(h->fd, VIDIOC_DQBUF, &v4l2_buf);
		}
	}

	buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ioctl(h->fd, VIDIOC_STREAMOFF, &buf_type);

	c_kfree(-1, ubuf.buf[0].vaddr);
	free(ubuf.buf);

	return 0;
}

static inline char *get_filename_extension(char *filename)
{
	char *ext = filename + strlen(filename);
	while( *ext != '.' && ext > filename )
		ext--;
	ext += *ext == '.';
	return ext;
}

static int v4l2_get_colorspace(char *input_filename, cli_opt_t *opt)
{
	char *ext = get_filename_extension(input_filename);
	if (strcmp(ext, "nv12") == 0) {
		opt->i_csp = C_CSP_NV12;
	} else {
		fprintf(stderr, "can't supported input file color space:%s\n", ext);
		goto err_unsupport_csp;
	}
	return 0;

err_unsupport_csp:
	return -1;
}

static int register_sensor_info(nv12_hnd_t *h)
{
	int ret = 0, temp = 0;
	struct v4l2_input input;

	switch(h->sensor_type) {
		case SENSOR_OV9712:
			memcpy(h->sensor_info.name, "ov9712", sizeof("ov9712"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "ov9712", sizeof("ov9712"));
			h->sensor_info.i2c.addr = 0x30;
			break;
		case SENSOR_OV9732:
			memcpy(h->sensor_info.name, "ov9732", sizeof("ov9732"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "ov9732", sizeof("ov9732"));
			h->sensor_info.i2c.addr = 0x36;
			break;
		case SENSOR_OV9750:
			memcpy(h->sensor_info.name, "ov9750", sizeof("ov9750"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "ov9750", sizeof("ov9750"));
			h->sensor_info.i2c.addr = 0x36;
			break;
		case SENSOR_AR0141:
			memcpy(h->sensor_info.name, "ar0141", sizeof("ar0141"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "ar0141", sizeof("ar0141"));
			h->sensor_info.i2c.addr = 0x10;
			break;
		case SENSOR_GC1004:
			memcpy(h->sensor_info.name, "gc1004", sizeof("gc1004"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "gc1004", sizeof("gc1004"));
			h->sensor_info.i2c.addr = 0x3c;
			break;
		case SENSOR_JXH42:
			memcpy(h->sensor_info.name, "jxh42", sizeof("jxh42"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "jxh42", sizeof("jxh42"));
			h->sensor_info.i2c.addr = 0x30;
			break;
		case SENSOR_SC1035:
			memcpy(h->sensor_info.name, "sc1035", sizeof("sc1035"));
			h->sensor_info.cbus_type = TX_SENSOR_CONTROL_INTERFACE_I2C;
			memcpy(h->sensor_info.i2c.type, "sc1035", sizeof("sc1035"));
			h->sensor_info.i2c.addr = 0x30;
			break;
		default:
			fprintf(stderr, "unsupported sensor type:%d\n", h->sensor_type);
			return -1;
	}

	if ((h->vi_fd = open("/dev/v4l-subdev0", O_RDWR)) < 0) {
		fprintf(stderr, "cannot open %s:%s\n", "/dev/v4l-subdev0", strerror(errno));
		goto err_open_v4l_subdev0;
	}

	if (ioctl(h->vi_fd, VIDIOC_REGISTER_SENSOR, &h->sensor_info)) {
		fprintf (stderr, "VIDIOC_REGISTER_SENSOR(%s) error!\n", h->sensor_info.name);
		goto err_VIDIOC_REGISTER_SENSOR;
	}

	/* enum input */
	input.index = 0;
	printf("enum sensor:\n");
	while ((ret = ioctl(h->vi_fd, VIDIOC_ENUMINPUT, &input)) == 0) {
		printf("index = %d  name = %s\n", input.index, input.name);
		input.index++;
	}

	if (ret != -1) {
		goto err_enum_input;
	} else {
		printf("cannot enum the sensor(index = %d)\n", input.index);
	}

	/* set input */
	temp = 0;
	if (ioctl(h->vi_fd, VIDIOC_S_INPUT, &temp) != 0) {
		printf("sorry,failed to set input(sensor index = %d)\n", temp);
		goto err_s_input;
	}

	if (ioctl(h->vi_fd, VIDIOC_STREAMON, &temp) != 0) {
		fprintf (stderr, "%s: VIDIOC_STREAMON failed\n", h->sensor_info.name);
		goto err_VIDIOC_STREAMON;
	}

	sleep(3);

	return 0;
err_VIDIOC_STREAMON:
	temp = -1;
	ioctl(h->vi_fd, VIDIOC_S_INPUT, &temp);
err_s_input:
err_enum_input:
	ioctl(h->vi_fd, VIDIOC_RELEASE_SENSOR, &h->sensor_info);
err_VIDIOC_REGISTER_SENSOR:
	close(h->vi_fd);
err_open_v4l_subdev0:
	return -1;
}

static void release_sensor_info(nv12_hnd_t *h)
{
	int temp = -1;
	ioctl(h->vi_fd, VIDIOC_STREAMOFF, &temp);
	ioctl(h->vi_fd, VIDIOC_S_INPUT, &temp);
	ioctl(h->vi_fd, VIDIOC_RELEASE_SENSOR, &h->sensor_info);
	close(h->vi_fd);
}

static int v4l2_open_file(char *filename, cli_opt_t *opt)
{
	nv12_hnd_t *h = NULL;
	char devname[64];

	h = malloc(sizeof(nv12_hnd_t));
	if (NULL == h) {
		fprintf(stderr, "malloc nv12_hnd_t failed.\n");
		goto err_malloc;
	}
	memset(h, 0, sizeof(nv12_hnd_t));

	if (opt->input_file_name != 0) {
		/* try to parse the file name */
		char *p = 0;
		for (p = opt->input_file_name; *p; p++) {
			if(*p >= '0' && *p <= '9'
				&& sscanf( p, "%dx%d", &opt->image_width, &opt->image_height) == 2) {
				break;
			}
		}
		if (v4l2_get_colorspace(opt->input_file_name, opt) < 0) {
			fprintf(stderr, "v4l2_get_colorspace failed\n");
			goto err_get_colorspace;
		}
	}

	if (!opt->image_width || !opt->image_height) {
		fprintf(stderr, "v4l2 input requires a resolution.\n");
		goto err_resolution;
	}
	h->width = opt->image_width;
	h->height = opt->image_height;
	h->next_frame = 0;
	h->i_csp = opt->i_csp;
	h->sensor_type = opt->sensor_type;
	h->chn_index = 0;

	if (register_sensor_info(h) < 0) {
		fprintf(stderr, "register_sensor_info failed\n");
		goto err_register_sensor_info;
	}

	if (v4l2_3d_init(h) < 0) {
		fprintf(stderr, "v4l2_3d_init failed\n");
		goto err_v4l2_3d_init;
	}

	sprintf(devname, "/dev/video%d", (h->chn_index + 1)%3);
	if ((h->fd = open(devname, O_RDWR)) < 0) {
		fprintf(stderr, "can't open %s\n", devname);
		goto err_open_dev;
	}

	if (v4l2_para_init(h, devname) < 0) {
		fprintf(stderr, "v4l2_para_init failed\n");
		goto err_para_init;
	}
	if (v4l2_init_ubuf(h) < 0) {
		fprintf(stderr, "v4l2_init_ubuf failed\n");
		goto err_get_ubuf;
	}
	if (opt->totalframe == 0)
		opt->totalframe = 200;
	opt->hin = (hnd_t)h;

	return 0;

err_get_ubuf:
err_para_init:
	close(h->fd);
err_open_dev:
	v4l2_3d_deinit(h);
err_v4l2_3d_init:
	release_sensor_info(h);
err_register_sensor_info:
err_resolution:
err_get_colorspace:
	free(h);
err_malloc:
	return -1;
}

static int v4l2_read_frame(i264e_pic_t *pic, cli_opt_t *opt)
{
	nv12_hnd_t *h = (nv12_hnd_t *)opt->hin;
	struct v4l2_buffer buf;
	int ready_bufs = 0;

	memset(&buf, 0, sizeof(struct v4l2_buffer));
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (ioctl(h->fd, VIDIOC_DEFAULT_CMD_LISTEN_BUF, &ready_bufs) < 0) {
		fprintf(stderr, "VIDIOC_DEFAULT_CMD_LISTEN_BUF failed:%s\n", strerror(errno));
		goto err_VIDIOC_DEFAULT_CMD_LISTEN_BUF;
	}

	if (ioctl(h->fd, VIDIOC_DQBUF, &buf) != 0) {
		fprintf(stderr, "VIDIOC_DQBUF failed:%s\n", strerror(errno));
		goto err_VIDIOC_DQBUF;
	}
	pic->img.alloc_plane[0] = pic->img.plane[0] = ubuf.buf[buf.index].vaddr;
	pic->img.alloc_plane[1] = pic->img.plane[1] = ubuf.buf[buf.index].vaddr + h->width * h->height;
	h->next_frame++;

	return 0;

err_VIDIOC_DQBUF:
err_VIDIOC_DEFAULT_CMD_LISTEN_BUF:
	return -1;
}

static int v4l2_close_file(cli_opt_t *opt)
{
	nv12_hnd_t *h = (nv12_hnd_t *)opt->hin;

	v4l2_deinit_ubuf(h);
	close(h->fd);
	v4l2_3d_deinit(h);
	release_sensor_info(h);
	free(h);

	return 0;
}

static int v4l2_picture_alloc(i264e_pic_t *pic, cli_opt_t *opt)
{
	int i = 0;
	memset(pic, 0, sizeof(*pic));
	pic->img.i_csp = opt->i_csp;
	pic->img.i_plane = 2;
	pic->i_pic_struct = C_PIC_STRUCT_AUTO;

	if (pic->img.i_csp == C_CSP_NV12) {
		pic->img.i_stride[0] = C_ALIGN(opt->image_width, 16);
		pic->img.i_stride[1] = C_ALIGN(opt->image_width, 16);
		pic->img.i_lines[0] = C_ALIGN(opt->image_height, 16);
		pic->img.i_lines[1] = C_ALIGN(opt->image_height, 16) >> 1;
	} else if (pic->img.i_csp == C_CSP_T420) {
		pic->img.i_stride[0] = C_ALIGN(opt->image_width, 16)*16;
		pic->img.i_stride[1] = C_ALIGN(opt->image_width, 16)*8;
		pic->img.i_lines[0] = C_ALIGN(opt->image_height, 16)>>4;
		pic->img.i_lines[1] = C_ALIGN(opt->image_height, 16)>>4;
	} else if (pic->img.i_csp == C_CSP_NV21) {
		pic->img.i_stride[0] = C_ALIGN(opt->image_width, 16);
		pic->img.i_stride[1] = C_ALIGN(opt->image_width, 16);
		pic->img.i_lines[0] = C_ALIGN(opt->image_height, 16);
		pic->img.i_lines[1] = C_ALIGN(opt->image_height, 16) >> 1;
	} else if (pic->img.i_csp == C_CSP_I420) {
		pic->img.i_plane = 3;
		pic->img.i_stride[0] = C_ALIGN(opt->image_width, 16);
		pic->img.i_stride[1] = C_ALIGN(opt->image_width, 16)>>1;
		pic->img.i_stride[2] = C_ALIGN(opt->image_width, 16)>>1;
		pic->img.i_lines[0] = C_ALIGN(opt->image_height, 16);
		pic->img.i_lines[1] = C_ALIGN(opt->image_height, 16)>>1;
		pic->img.i_lines[2] = C_ALIGN(opt->image_height, 16)>>1;
	}
	for (i = 0; i < pic->img.i_plane; i++) {
		pic->img.alloc_plane[i] = NULL;
		pic->img.plane[i] = NULL;
		pic->img.is_vir = opt->b_use_phy;
	}
	return 0;
}

static void v4l2_picture_clean(i264e_pic_t *pic, cli_opt_t *opt)
{
}

static int v4l2_release_frame(i264e_pic_t *pic, cli_opt_t *opt)
{
	int i = 0;
	nv12_hnd_t *h = (nv12_hnd_t *)opt->hin;

	for (i = 0; i < ubuf.buf_cnt; i++) {
		if(ubuf.buf[i].vaddr == pic->img.plane[0]) {
			struct v4l2_buffer v4l2_buf;
			memset(&v4l2_buf, 0, sizeof(struct v4l2_buffer));
			v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			v4l2_buf.memory = V4L2_MEMORY_USERPTR;
			v4l2_buf.index = i;
			v4l2_buf.m.userptr = (unsigned long)(ubuf.buf[i].paddr);
			v4l2_buf.length = ubuf.buf[i].len;
			if (ioctl(h->fd, VIDIOC_QBUF, &v4l2_buf) < 0) {
				fprintf(stderr, "VIDIOC_QBUF buf%d failed:%s\n", i, strerror(errno));
				goto err_VIDIOC_QBUF;
			}
			break;
		}
	}
	if (i == ubuf.buf_cnt) {
		fprintf(stderr, "no qbuf found\n");
		goto err_no_qbuf;
	}

	return 0;

err_no_qbuf:
err_VIDIOC_QBUF:
	return -1;
}

const cli_input_t cli_input_v4l2 = { v4l2_open_file, v4l2_picture_alloc, v4l2_read_frame, v4l2_release_frame, v4l2_picture_clean, v4l2_close_file };
